#include "arm_math.h"
#include "board.h"
#include "dji.h"
#include "imu.h"
#include "message.h"
#include "motor_def.h"
#include "offline.h"
#include "robotdef.h"
#include "rtconfig.h"
#include "rttypes.h"
#include "jiuyuan.h"
#include <math.h>
#include <stdlib.h>

#define LOG_TAG              "gimcmd"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>

static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息
DJIMotor_t  *right_wheel, *left_wheel; // 拨盘电机


#define DEV_PWM_NAME        "pwm1"  /* PWM设备名称 */
#define SERVO_VISION_CHANNEL       1   /* PWM通道 */
#define SERVO_GRAB_CHANNEL         2   /* PWM通道 */

struct rt_device_pwm *dev_servo;      /* PWM设备句柄 */

float target_yaw_angle = 0.0f; // 目标yaw角度
float current_yaw_angle = 0.0f; // 当前yaw角度       
static float yaw_error,left_speed,right_speed;     
static rt_uint8_t init_flag=0,init_first_flag=0;

#define WHEEL_MAX_SPEED  ((PI * 0.085 * 8000)/(60 *36)) 
#define YAW_DEAD_ZONE 5.0f // 死区范围，单位为度
#define XY_DEAD_ZONE 30// XY死区范围，单位为像素
#define CENTER_DISTANCE 1.0f // 中心点前进距离，单位为米
#define GRAB_TIME_THRESHOLD 1000 // 抓取时间阈值，单位为毫秒

void gimbal_init(void)
{
    // 左轮
    Motor_Init_Config_s friction_config = {
        .offline_manage_motor ={
            .event = OFFLINE_SHOOT_MOTOR_LEFT,
            .enable = OFFLINE_ENABLE,
            .online_state = STATE_OFFLINE,
            .error_level =OFFLINE_ERROR_LEVEL,
            .beep_times = 1,
            .last_time =rt_tick_get(),
            .offline_time =500,
        },
        .can_init_config = {
                            .can_handle = &can1_bus,
                            },
        .controller_param_init_config = {
                            .lqr_config ={
                                .K ={0.008f}, //0.0317
                                .output_max = 1.8f,
                                .output_min =-1.8f,
                                .state_dim = 1,
                            }
        },
        .controller_setting_init_config = {
                            .angle_feedback_source = MOTOR_FEED,
                            .speed_feedback_source = MOTOR_FEED,
                            .outer_loop_type    = SPEED_LOOP,
                            .close_loop_type    = SPEED_LOOP,
                            .feedback_reverse_flag =FEEDBACK_DIRECTION_NORMAL,
                            .control_algorithm =CONTROL_LQR,
                            .Feedback_Object_Type =MOTOR_ROTOR_FEEDBACK,
        },
        .motor_type = M2006
    };
    friction_config.can_init_config.tx_id = 2,
    left_wheel                            = DJIMotorInit(&friction_config);

    friction_config.can_init_config.tx_id                             = 1; // 右摩擦轮,改txid和方向就行
    friction_config.offline_manage_motor.event                        = OFFLINE_SHOOT_MOTOR_RIGHT;
    friction_config.offline_manage_motor.beep_times                   = 2;
    right_wheel                                                        = DJIMotorInit(&friction_config);
    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
}


rt_int16_t speed_to_rpm(float v){
    rt_int16_t rpm = (v * 60 * 36)/(PI * 0.085);
    return rpm;
}

float y_to_yaw(int y)
{
    //相机视野高度为320像素，yaw范围为-30度到30度
    float yaw = (y - 160) * (30.0f / 160.0f);
    return yaw;
}

float x_to_distance(int x)
{
    // 相机视野宽度为240像素，每像素对应0.0003米
    float distance = x * 0.0003f;
    return distance;
}

void ServoSetAngle(rt_uint8_t servo, rt_uint8_t action)
{
    if (servo == SERVO_VISION_CHANNEL) 
    {
        if (action == 0) 
        {
            rt_pwm_set(dev_servo, SERVO_VISION_CHANNEL, 20000000, 1850000);
        }
        else 
        {
            rt_pwm_set(dev_servo, SERVO_VISION_CHANNEL, 20000000, 2080000);
        }
    }
    else 
    {
        if (action == 0) 
        {
            rt_pwm_set(dev_servo, SERVO_GRAB_CHANNEL, 20000000, 1300000);
        }
        else 
        {
            rt_pwm_set(dev_servo, SERVO_GRAB_CHANNEL, 20000000, 1600000);
        }
    }
}

void move_forward(float distance)
{
    // 根据距离计算速度
    float speed = distance / 2.0f; // 2秒内完成移动
    left_speed = speed_to_rpm(LIMIT_MIN_MAX(speed, -0.2, 0.2)) * 6.0f;
    right_speed = (-speed_to_rpm(LIMIT_MIN_MAX(speed, -0.2, 0.2))) * 6.0f;

    DJIMotorSetRef(left_wheel, left_speed);
    DJIMotorSetRef(right_wheel, right_speed);
}

void turn_to_yaw(float target_yaw)
{
        current_yaw_angle = INS.Yaw;
        yaw_error = target_yaw - current_yaw_angle;

        if (fabs(yaw_error) < 2) {
            yaw_error = 0;
        }

        float tmp = -yaw_error * 0.01;
        left_speed = speed_to_rpm(LIMIT_MIN_MAX(tmp, -0.1, 0.1)) * 6.0f;
        right_speed = speed_to_rpm(LIMIT_MIN_MAX(tmp, -0.1, 0.1)) * 6.0f;

        DJIMotorSetRef(left_wheel, left_speed);
        DJIMotorSetRef(right_wheel, right_speed);
}


static void gimbal_thread_entry(void *parameter)
{   
    rt_uint16_t grab_time = 0;
    rt_uint16_t get_time  = 0;
    rt_thread_mdelay(2000);
    gimbal_init();

    while (1)
    {
        SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
        current_yaw_angle = INS.Yaw;

        if (offline_manage[SYSTEM_PROTECT].online_state == STATE_ONLINE) 
        {
            DJIMotorEnable(left_wheel);
            DJIMotorEnable(right_wheel);
            if (init_flag == 0) 
            {
                ServoSetAngle(SERVO_VISION_CHANNEL,0);
                ServoSetAngle(SERVO_GRAB_CHANNEL,0);
                left_speed = speed_to_rpm(0.5) * 6.0f;
                right_speed = speed_to_rpm(-0.5) * 6.0f;
                DJIMotorSetRef(left_wheel, left_speed);
                DJIMotorSetRef(right_wheel, right_speed);
                rt_thread_mdelay(2000);
                init_flag=1;
            }
            else 
            {
                if (init_first_flag == 0) 
                {
                    int x = JiuYuan.redblue_x;
                    int y = JiuYuan.redblue_y;
    
                    if (JiuYuan.redblueid == 0XC0) 
                    {
                        if (x !=0 && y!= 0) 
                        {
                            if (abs(x)>XY_DEAD_ZONE) 
                            { 
                                if (x >0) 
                                {
                                    left_speed = speed_to_rpm(0.02) * 6.0f;
                                    right_speed = speed_to_rpm(0.02) * 6.0f;
                                    DJIMotorSetRef(left_wheel, left_speed);
                                    DJIMotorSetRef(right_wheel, right_speed);
                                }
                                else 
                                {
                                    left_speed = speed_to_rpm(-0.02) * 6.0f;
                                    right_speed = speed_to_rpm(-0.02) * 6.0f;
                                    DJIMotorSetRef(left_wheel, left_speed);
                                    DJIMotorSetRef(right_wheel, right_speed);
                                }
                            }
                            else if (abs(y)>XY_DEAD_ZONE) 
                            {
                                if (y>0) 
                                {
                                    left_speed = speed_to_rpm(0.1) * 6.0f;
                                    right_speed = speed_to_rpm(-0.1) * 6.0f;
                                    DJIMotorSetRef(left_wheel, left_speed);
                                    DJIMotorSetRef(right_wheel, right_speed);
                                }
                                // else 
                                // {
                                //     left_speed = speed_to_rpm(-0.1) * 6.0f;
                                //     right_speed = speed_to_rpm(0.1) * 6.0f;
                                //     DJIMotorSetRef(left_wheel, left_speed);
                                //     DJIMotorSetRef(right_wheel, right_speed);
                                // }
                            }   
                            
                            if  (abs(x)<XY_DEAD_ZONE && y< 0)  
                            {
                                grab_time++;
                                if (grab_time > GRAB_TIME_THRESHOLD) 
                                {
                                    send_jiuyuan(1);
                                    ServoSetAngle(SERVO_GRAB_CHANNEL,1);
                                    ServoSetAngle(SERVO_VISION_CHANNEL,1);
                                }
                            }
                        }
                        else 
                        {
                            left_speed = speed_to_rpm(0.1) * 6.0f;
                            right_speed = speed_to_rpm(0.1) * 6.0f;
                            DJIMotorSetRef(left_wheel, left_speed);
                            DJIMotorSetRef(right_wheel, right_speed);
                        }
                    }
                    if (JiuYuan.redblueid == 0XB0) 
                    {
                        if (x !=0 && y!= 0) 
                        {
                            if (abs(x)>XY_DEAD_ZONE) 
                            { 
                                if (x >0) 
                                {
                                    left_speed = speed_to_rpm(0.02) * 6.0f;
                                    right_speed = speed_to_rpm(0.02) * 6.0f;
                                    DJIMotorSetRef(left_wheel, left_speed);
                                    DJIMotorSetRef(right_wheel, right_speed);
                                }
                                else 
                                {
                                    left_speed = speed_to_rpm(-0.02) * 6.0f;
                                    right_speed = speed_to_rpm(-0.02) * 6.0f;
                                    DJIMotorSetRef(left_wheel, left_speed);
                                    DJIMotorSetRef(right_wheel, right_speed);
                                }
                            }
                            else if (abs(y)>XY_DEAD_ZONE) 
                            {
                                if (y>0) 
                                {
                                    left_speed = speed_to_rpm(0.1) * 6.0f;
                                    right_speed = speed_to_rpm(-0.1) * 6.0f;
                                    DJIMotorSetRef(left_wheel, left_speed);
                                    DJIMotorSetRef(right_wheel, right_speed);
                                }
                                else 
                                {
                                    left_speed = speed_to_rpm(-0.1) * 6.0f;
                                    right_speed = speed_to_rpm(0.1) * 6.0f;
                                    DJIMotorSetRef(left_wheel, left_speed);
                                    DJIMotorSetRef(right_wheel, right_speed);
                                }
                            }      
                            
                            if  (abs(x)<XY_DEAD_ZONE && y< 0)  
                            {
                                get_time++;
                                if (get_time > GRAB_TIME_THRESHOLD) 
                                {
                                    ServoSetAngle(SERVO_GRAB_CHANNEL,0);
                                    left_speed = speed_to_rpm(-0.1) * 6.0f;
                                    right_speed = speed_to_rpm(-0.1) * 6.0f;
                                    DJIMotorSetRef(left_wheel, left_speed);
                                    DJIMotorSetRef(right_wheel, right_speed);
                                    rt_thread_mdelay(500);
                                    init_first_flag =1;
                                    grab_time =0;
                                    get_time  =0;
                                    send_jiuyuan(0);
                                }
                            }
                        }
                        else 
                        {
                            left_speed = speed_to_rpm(0.1) * 6.0f;
                            right_speed = speed_to_rpm(0.1) * 6.0f;
                            DJIMotorSetRef(left_wheel, left_speed);
                            DJIMotorSetRef(right_wheel, right_speed);
                        }
                    }
                    
                }
                else 
                {
                    if (JiuYuan.black_x !=0 || JiuYuan.black_y !=0 || JiuYuan.redblue_x !=0 || JiuYuan.redblue_y !=0 && JiuYuan.redblueid == 0XB0) 
                    {
                        if ( sqrtf((JiuYuan.redblue_x) * (JiuYuan.redblue_x)+ (JiuYuan.redblue_y) * (JiuYuan.redblue_y)) 
                        < sqrtf((JiuYuan.black_x) * (JiuYuan.black_x)+ (JiuYuan.black_y) * (JiuYuan.black_y)) ) 
                        {
                            if (JiuYuan.redblue_x !=0 && JiuYuan.redblue_y!= 0) 
                            {
                                if (abs(JiuYuan.redblue_x)>XY_DEAD_ZONE) { target_yaw_angle = y_to_yaw(JiuYuan.redblue_x);turn_to_yaw(-target_yaw_angle);}
                                else if (abs(JiuYuan.redblue_y)>XY_DEAD_ZONE) {float distance_to_move = x_to_distance(JiuYuan.redblue_y);move_forward(distance_to_move);}   
                                
                                if  (abs(JiuYuan.redblue_x)<XY_DEAD_ZONE && abs(JiuYuan.redblue_y)< XY_DEAD_ZONE)  
                                {
                                    get_time++;
                                    if (get_time > GRAB_TIME_THRESHOLD) 
                                    {
                                        ServoSetAngle(SERVO_GRAB_CHANNEL,0);
                                        left_speed = speed_to_rpm(-0.1) * 6.0f;
                                        right_speed = speed_to_rpm(-0.1) * 6.0f;
                                        DJIMotorSetRef(left_wheel, left_speed);
                                        DJIMotorSetRef(right_wheel, right_speed);
                                        rt_thread_mdelay(500);
                                        init_first_flag =1;
                                        grab_time =0;
                                        get_time  =0;
                                    }
                                }
                            }
                            else 
                            {
                                left_speed = speed_to_rpm(0.1) * 6.0f;
                                right_speed = speed_to_rpm(0.1) * 6.0f;
                                DJIMotorSetRef(left_wheel, left_speed);
                                DJIMotorSetRef(right_wheel, right_speed);
                            }
                        }
                        else 
                        {
                            if (abs(JiuYuan.black_x)>XY_DEAD_ZONE) { target_yaw_angle = y_to_yaw(JiuYuan.black_x);turn_to_yaw(-target_yaw_angle);}
                            else if (abs(JiuYuan.black_y)>XY_DEAD_ZONE) {float distance_to_move = x_to_distance(JiuYuan.black_y);move_forward(distance_to_move);}   
                                
                            if  (abs(JiuYuan.black_x)<XY_DEAD_ZONE && abs(JiuYuan.black_y)< XY_DEAD_ZONE)  
                            {
                                get_time++;
                                if (get_time > GRAB_TIME_THRESHOLD) 
                                {
                                        ServoSetAngle(SERVO_GRAB_CHANNEL,0);
                                        left_speed = speed_to_rpm(-0.1) * 6.0f;
                                        right_speed = speed_to_rpm(-0.1) * 6.0f;
                                        DJIMotorSetRef(left_wheel, left_speed);
                                        DJIMotorSetRef(right_wheel, right_speed);
                                        rt_thread_mdelay(500);
                                        grab_time =0;
                                        get_time  =0;
                                }
                            }
                        }
                    }
                    else 
                    {
                        left_speed = speed_to_rpm(0.1) * 6.0f;
                        right_speed = speed_to_rpm(0.1) * 6.0f;
                        DJIMotorSetRef(left_wheel, left_speed);
                        DJIMotorSetRef(right_wheel, right_speed);
                    }
                }

            }
        }
        else 
        {
            DJIMotorStop(left_wheel);
            DJIMotorStop(right_wheel);
        }
        rt_thread_mdelay(2);
    }

}
int gimbal_thread_entry_init(void)
{
    rt_err_t ret = RT_EOK;
    rt_thread_t thread = rt_thread_create("gimbal", gimbal_thread_entry, RT_NULL, 4096, 30, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        ret = RT_ERROR;
    }

    LOG_D("gimbal thread init success!\n");

    /* 查找设备 */
    dev_servo = (struct rt_device_pwm *)rt_device_find(DEV_PWM_NAME);
    if (dev_servo == RT_NULL)
    {
        LOG_E("pwm sample run failed! can't find %s device!\n", DEV_PWM_NAME);
        ret = -RT_ERROR;
    }
    
    rt_pwm_enable(dev_servo, SERVO_VISION_CHANNEL);
    rt_pwm_enable(dev_servo, SERVO_GRAB_CHANNEL);

    return ret;
}
INIT_APP_EXPORT(gimbal_thread_entry_init);


